//
// File: rfnn_to_vel.cpp
//
// MATLAB Coder version            : 5.4
// C/C++ source code generated on  : 16-Dec-2024 10:45:09
//

// Include Files
#include "rfnn_to_vel.h"
#include <cmath>

// Function Definitions
//
// Arguments    : const double Outputs[2]
//                double b
//                double aph
//                double beta
//                double *vR
//                double *vL
// Return Type  : void
//
void rfnn_to_vel(const double Outputs[2], double b, double aph, double beta,
                 double *vR, double *vL)
{
  // 最小转向半径
  *vR = Outputs[0] / (beta + aph * std::abs(Outputs[0]));
  *vL = Outputs[1] / (beta + aph * std::abs(Outputs[1]));
  if (std::abs((*vR + *vL) / 2.0 / ((*vR - *vL) / (2.0 * b))) < 0.165) {
    if (*vR > *vL) {
      *vR = b * 1.8181818181818181 + 0.3;
      *vL = 0.3 - b * 1.8181818181818181;
    } else {
      *vR = b * -1.8181818181818181 + 0.3;
      *vL = 0.3 - b * -1.8181818181818181;
    }
  }
}

//
// File trailer for rfnn_to_vel.cpp
//
// [EOF]
//
